#include <iostream>
#include "VehicleStateManager.h"
#include "EstimationStateManager.h"

using namespace std;



VehicleStateManager::VehicleStateManager(SqLiteDbWrapper* connection, int RouteID){
            //Sets default state before Journey begins

            db = connection;
            pathList = db->LoadRoute(RouteID);

            changeState(ATE_STATE_OUT_OF_SERVICE);
            //TODO: Grab pointRadius from a config file?
            #define POINT_RADIUS 0.0002

            //Set the route marker to the first path in the route
            listIterator = pathList.begin();
            currentNode = db->GetNode(listIterator->startNode);


            //Route is inferred as circular by comparing very first and last nodes
            //This is validated when creating a route using the RouteDesigner
            routeIsCircular = true;/*(pathList.front().startNode == pathList.back().endNode);*/
            cout << "Route is" << ((routeIsCircular)?" ":" not ") << "circular\n";

            EstimationStateManager esm(&State, &gps.odo, db, &pathList, RouteID);

            //Launch asynchronous Estimation State Manager on a seperate thread
            //Object is passed by value thus not destructed once scope is lost
            boost::thread esmThread(esm);


            nodeListenLoop();
}

bool VehicleStateManager::setNode(){//Moves iterator on by one until end is reached,
                                    //where it is returned to first path or flags that
                                    //the state machine should halt when the next node
                                    //is reached
    ++listIterator;


    if (listIterator == pathList.end()){

        if(!routeIsCircular){
            currentNode = db->GetNode(pathList.back().endNode);
            return true;
        }
        else
            listIterator = pathList.begin();
    }

    currentNode = db->GetNode(listIterator->startNode);
    return false;

}

void VehicleStateManager::nodeListenLoop(){//Method to move between states while journey is progress
                                           //and signal to ESM when the state has changed


    bool routeEnd = false;

    //State Manager will not reach a halting state if the route is specified as circular
    while (!routeEnd){
        boost::posix_time::microseconds halfsec(333);
        boost::this_thread::sleep(halfsec);

        //Read GPS Stream
        gps.GetSquareLatLong(gpsBuffer);


        //Once we arrive at a node, keep reading the GPS stream until we've left
        if (atNextNode()){
            cout << "Node " << currentNode.name << " has been reached!\n";
            changeState(ATE_STATE_AT_NODE);

            while (atNextNode()){
                boost::this_thread::sleep(halfsec);
                gps.GetSquareLatLong(gpsBuffer);
            }

            cout << "Bus has departed node " << currentNode.name << "!\n";
            routeEnd = setNode();
        }

        changeState(ATE_STATE_ON_LINK);
    }

    lastNodeLoop();
}

bool VehicleStateManager::atNextNode(){//compare the location of the vehicle
                                       //against the location of the next stop
    return ((gpsBuffer[0] < currentNode.latitude + POINT_RADIUS && gpsBuffer[0] > currentNode.latitude - POINT_RADIUS)
                    && (gpsBuffer[1] < currentNode.longitude + POINT_RADIUS && gpsBuffer[1] > currentNode.longitude - POINT_RADIUS));
}

void VehicleStateManager::lastNodeLoop(){//Waits for contact with last node before closing state machine
                                         //and finishing session
    boost::posix_time::microseconds halfsec(333);

    while (!atNextNode()){
        boost::this_thread::sleep(halfsec);
        gps.GetSquareLatLong(gpsBuffer);
    }

    changeState(ATE_STATE_OUT_OF_SERVICE);
    cout << "You have reached your destination: " << currentNode.name << "\a\n";


}

void VehicleStateManager::changeState(VehicleStates newState){//Setter method to ensure State is thread safe,
                                                              //and prevent unaccesible memory errors
    boost::mutex mutex;
    boost::mutex::scoped_lock lock(mutex);

    State = newState;

}

VehicleStateManager::~VehicleStateManager(){//Before closing the session, sleep until ESM has finished up
    esmThread.join();
    cout << "\nClosing Vehicle State Machine\n\n";
}
